use chrono::{DateTime, Utc};
use r2r::unitree_api::msg::Request;
use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep;
use std::time::Duration;
use unitree_ros2_rs::sport_client::client::SportClient;

static RUNNING: AtomicBool = AtomicBool::new(true);

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // 设置优雅的信号处理器
    ctrlc::set_handler(|| {
        println!("\nReceived Ctrl+C, stopping gracefully...");
        RUNNING.store(false, Ordering::SeqCst);
    })?;

    println!("Starting sport euler demo...");
    println!("Press Ctrl+C to stop the demo");

    // 创建sport client
    let sport_client = SportClient::new("sport_euler_demo", "")?;
    
    let start_time: DateTime<Utc> = Utc::now();
    let dt = 0.01; // 控制步长
    
    let mut req = Request::default();

    sport_client.RecoveryStand(&mut req);
    sport_client.BalanceStand(&mut req);
    
    while RUNNING.load(Ordering::SeqCst) {
        let elapsed = Utc::now().signed_duration_since(start_time);
        let t = elapsed.num_milliseconds() as f64 / 1000.0; // 转换为秒
        
        // 姿态跟踪一个三角函数轨迹
        let roll = 0.2 * (2.0 * t).sin();
        let pitch = 0.2 * (2.0 * t).cos() - 0.2;
        let yaw = 0.0;
        
        println!("Time: {:.2}s, Roll: {:.3}, Pitch: {:.3}, Yaw: {:.3}", t, roll, pitch, yaw);
        
        // 发送姿态控制命令
        sport_client.Euler(&mut req, roll as f32, pitch as f32, yaw as f32);
        sport_client.BalanceStand(&mut req);
        
        // 等待控制步长时间
        sleep(Duration::from_millis((dt * 1000.0) as u64));
    }
    
    // 程序退出时复位姿态
    println!("Resetting pose to neutral position...");
    sport_client.Euler(&mut req, 0.0, 0.0, 0.0);
    sport_client.BalanceStand(&mut req);
    
    println!("Sport euler demo finished.");
    
    Ok(())
}